#include "roombalib.h"
#include "opencv/highgui.h"
#include "opencv/cv.h"
#include "ivy.h"
#include "ivyloop.h"
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <getopt.h>
#include <string.h>
#include <time.h>
#include <math.h>

void marker ( IplImage* image, CvPoint objectNextPos, int nbPixels );
CvPoint binarisation ( IplImage* image, int *nbPixels );
int roombamain ( int id );
void retrieveMSG ( IvyClientPtr app, void *data, int argc, char **argv );

#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define sign(x) ((x) > 0 ? 1 : -1)
#define STEP_MIN 5
#define STEP_MAX 100

IplImage *image;
CvPoint objectPos = cvPoint(-1, -1);
int h=19, s=191, v=93, tolerance = 5;

CvPoint binarisation(IplImage* image, int *nbPixels) {
	int x, y;
	CvScalar pixel;
	IplImage *hsv, *mask;
	IplConvKernel *kernel;
	int sommeX = 0, sommeY = 0;
	*nbPixels = 0;
	mask = cvCreateImage(cvGetSize(image), image->depth, 1);
	hsv = cvCloneImage(image);
	cvCvtColor(image, hsv, CV_BGR2HSV);
	cvInRangeS(hsv, cvScalar(h - tolerance -1, s - tolerance, 0), cvScalar(h + tolerance -1, s + tolerance, 255), mask);
	kernel = cvCreateStructuringElementEx(5, 5, 2, 2, CV_SHAPE_ELLIPSE);
	cvDilate(mask, mask, kernel, 1);
	cvErode(mask, mask, kernel, 1);
	for(x = 0; x < mask->width; x++) {
		for(y = 0; y < mask->height; y++) {
			if(((uchar *)(mask->imageData + y*mask->widthStep))[x] == 255) {
				sommeX += x;
				sommeY += y;
				(*nbPixels)++;
			}
		}
	}
	cvReleaseStructuringElement(&kernel);
	cvReleaseImage(&mask);
   	cvReleaseImage(&hsv);
	if(*nbPixels > 0)
		return cvPoint((int)(sommeX / (*nbPixels)), (int)(sommeY / (*nbPixels)));
	else
		return cvPoint(-1, -1);
}
//Roomba* roomba = roomba_init( "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A100N34T-if00-port0", B115200 );
Roomba* roomba = roomba_init( "/dev/ttyUSB0", B115200 );

void marker ( IplImage* image, CvPoint objectNextPos, int nbPixels ) {
	int objectNextStepX, objectNextStepY;
	if (nbPixels > 10) {
		if (objectPos.x == -1 || objectPos.y == -1) {
			objectPos.x = objectNextPos.x;
			objectPos.y = objectNextPos.y;
		}
		if (abs(objectPos.x - objectNextPos.x) > STEP_MIN) {
			objectNextStepX = max(STEP_MIN, min(STEP_MAX, abs(objectPos.x - objectNextPos.x) / 2));
			objectPos.x += (-1) * sign(objectPos.x - objectNextPos.x) * objectNextStepX;
		}
		if (abs(objectPos.y - objectNextPos.y) > STEP_MIN) {
			objectNextStepY = max(STEP_MIN, min(STEP_MAX, abs(objectPos.y - objectNextPos.y) / 2));
			objectPos.y += (-1) * sign(objectPos.y - objectNextPos.y) * objectNextStepY;
		}
/**---------------------------------------------------------------Ligne1------------------------------------------------------------------------*/
if(objectPos.x >= 0 && objectPos.x <= (cvGetSize(image).width)/3 && objectPos.y >= 0 && objectPos.y <= (cvGetSize(image).height)/3 ){
	//DriveCase1
	roomba_drive( roomba, 50, 200 );
	roomba_delay(300);
}

else if(objectPos.x >= (cvGetSize(image).width)/3 && objectPos.x <= (cvGetSize(image).width)/1.5 && objectPos.y >= 0 && objectPos.y <= (cvGetSize(image).height)/3 ){
	//DriveCase2
	roomba_drive( roomba, 50, 0 );
	roomba_delay(270);
}

else if(objectPos.x >= (cvGetSize(image).width)/1.5 && objectPos.x <= (cvGetSize(image).width) && objectPos.y >= 0 && objectPos.y <= (cvGetSize(image).height)/3 ){
	//DriveCase3
	roomba_drive( roomba, 50, -200 );
	roomba_delay(300);
}
/**---------------------------------------------------------------Ligne2------------------------------------------------------------------------*/

else if(objectPos.x >= 0 && objectPos.x <= (cvGetSize(image).width)/3 && objectPos.y >= (cvGetSize(image).height)/3 && objectPos.y <= (cvGetSize(image).height)/1.5 ){
	//DriveCase4
	roomba_drive( roomba, 20, 0 );
	roomba_delay(250);
	roomba_drive( roomba, 20, 1 );
	roomba_delay(200);
}

else if(objectPos.x >= (cvGetSize(image).width)/3 && objectPos.x <= (cvGetSize(image).width)/1.5 && objectPos.y >= (cvGetSize(image).height)/3 && objectPos.y <= (cvGetSize(image).height)/1.5 ){
	//DriveCase5
	roomba_stop( roomba );
	uint8_t cmd[] = { 140, 1, 16, 
					31,		10,		//note 1
					32,		10,		//note 2
					33,		10,		//note 3
					34,		10,		//note 4
					35,		10,		//note 5
					36,		10,		//note 6
					37,		10,		//note 7
					38,		10,		//note 8
					38,		10,		//note 9
					37,		10,		//note 10
					36,		10,		//note 11
					35,		10,		//note 12
					34,		10,		//note 13
					33,		10,		//note 14
					32,		10,		//note 15
					31,		10		//note 16
					};
	roomba_send( roomba, cmd, 35 );
	uint8_t cmd2[] = { 141, 1 };
	roomba_send( roomba, cmd2, 2 );
	//Retour à la base
	//uint8_t cmd3[] = { 143 };
	//roomba_send( roomba, cmd3, 2 );
	}

else if(objectPos.x >= (cvGetSize(image).width)/1.5 && objectPos.x <= (cvGetSize(image).width) && objectPos.y >= (cvGetSize(image).height)/3 && objectPos.y <= (cvGetSize(image).height)/1.5 ){
	//DriveCase6
	roomba_drive( roomba, 20, 0 );
	roomba_delay(250);
	roomba_drive( roomba, 20, -1 );
	roomba_delay(200);
}
/**---------------------------------------------------------------Ligne3------------------------------------------------------------------------*/
else if(objectPos.x >= 0 && objectPos.x <= (cvGetSize(image).width)/3 && objectPos.y >= (cvGetSize(image).height)/1.5 && objectPos.y <= (cvGetSize(image).height) ){
	//DriveCase7
	roomba_drive( roomba, -50, 0 );
	roomba_delay(200);
}

else if(objectPos.x >= (cvGetSize(image).width)/3 && objectPos.x <= (cvGetSize(image).width)/1.5 && objectPos.y >= (cvGetSize(image).height)/1.5 && objectPos.y <= (cvGetSize(image).height) ){
	//DriveCase8
	roomba_drive( roomba, -50, 0 );
	roomba_delay(200);
}

else if(objectPos.x >= (cvGetSize(image).width)/1.5 && objectPos.x <= (cvGetSize(image).width) && objectPos.y >= (cvGetSize(image).height)/1.5 && objectPos.y <= (cvGetSize(image).height) ){
	//DriveCase9
	roomba_drive( roomba, -50, 0 );
	roomba_delay(200);
}
/**-------------------------------------------------------------DéplacementAléatoire------------------------------------------------------------*/
}
	else {
		objectPos.x = -1;
		objectPos.y = -1;
		int r=0;
		r = rand()%4;
		roomba_set_velocity( roomba, 25 );
		//while(i<10)
		switch (r) {
			case 0 :
				roomba_forward( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				//i++;
				break;
			case 1 :
				roomba_forward( roomba ); //roomba_backward( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				//i++;
				break;
			case 2 :
				roomba_spinleft( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				//i++;
				break;
			case 3 :
				roomba_spinright( roomba );
				roomba_delay(500);
				roomba_stop( roomba );
				//i++;
				break;
		}
	}
	if (nbPixels > 10)
		cvDrawCircle(image, objectPos, 15, CV_RGB(127, 127, 127), -1);

cvDrawLine(image,cvPoint((cvGetSize(image).width)/3,0),cvPoint((cvGetSize(image).width)/3,cvGetSize(image).height),CV_RGB(255,0,0),1,8,0);
cvDrawLine(image,cvPoint((cvGetSize(image).width)/1.5,0),cvPoint((cvGetSize(image).width)/1.5,cvGetSize(image).height),CV_RGB(255,0,0),1,8,0);
cvDrawLine(image,cvPoint(0,(cvGetSize(image).height)/3),cvPoint(cvGetSize(image).width,(cvGetSize(image).height)/3),CV_RGB(255,0,0),1,8,0);
cvDrawLine(image,cvPoint(0,(cvGetSize(image).height)/1.5),cvPoint(cvGetSize(image).width,(cvGetSize(image).height)/1.5),CV_RGB(255,0,0),1,8,0);


	cvShowImage("Suivi de couleur", image);
}

void get_couleur(int IdColor){
	switch (IdColor) {
		case 0 :
			h=76, s=64, v=61;		//vert
			break;
		case 1 :
			h=311, s=62, v=37;		//violet
			break;
		case 2 :
			h=45, s=43, v=69;		//jaune
			break;
		default :
			h=76, s=64, v=61;		//vert
	}
}

int roombamain(int id) {
	IplImage *hsv;
	CvCapture *capture;
	char key;
	int nbPixels;
	CvPoint objectNextPos;
 	capture = cvCreateCameraCapture(1);
   	if (!capture) {
   		printf("Impossible d'initialiser la capture vidéo.\n");
        return -1;
 	}
   	cvNamedWindow("Suivi de couleur", CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Suivi de couleur", 900, 900);
	//cvSetMouseCallback("Suivi de couleur", get_color);
	roombadebug = 1;
	time_t start,end;
	time(&start);
	int counter=0;
	//get_couleur(id);
	switch (id) {
		case 0 :
			h=19, s=191, v=93;		//vert
			break;
		case 1 :
			h=19, s=191, v=93;		//violet
			break;
		case 2 :
			h=19, s=191, v=93;		//jaune
			break;
		default :
			h=19, s=191, v=93;		//vert
	}
	while(key != 'Q' && key != 'q') {
		image = cvQueryFrame(capture);
		if(!image)
			continue;
		objectNextPos = binarisation(image, &nbPixels);
		marker(image, objectNextPos, nbPixels);
		time(&end);
		++counter;
		double sec=difftime(end,start);
		double fps=counter/sec;
		printf("\nfps = %lf\n",fps);
		key = cvWaitKey(10);
	}
	cvDestroyWindow("Suivi de couleur");
	cvReleaseCapture(&capture);
	return 0;
}

void retrieveMSG (IvyClientPtr app, void *data, int argc, char **argv){
	const char* arg = (argc < 1) ? "" : argv[0];
	printf("argv[0] : %s\n",argv[0]);
	int idcouleur;
	idcouleur = atoi (argv[0]);
	roombamain(idcouleur);
}

main (int argc, char**argv){
	const char* bus = 0;
	char c;
	while (c = getopt (argc, argv, "b:") != EOF) {
		switch (c) {
			case 'b':
				bus = optarg;
				break;
		}
	}
	if (!bus)
		bus = getenv ("IVYBUS");
	IvyInit ("roombavideo", "ICI_roombavideo", 0, 0, 0, 0);		//	initialisation
	IvyStart ("127.255.255.255:2010");							//	On lance l'agent sur le bus ivy
	IvyBindMsg (retrieveMSG, 0, "^S:(.*):(.*)$");				// on récupère tout les message commençant par S:Id_couleur:Qte_eau
	IvyMainLoop();												//	main loop
}
